function [u,info,oMPC] = MPC1xx_update(iMPC,feedback,x0)
% [x,fval] = MPC1xx_update(feedback,settings,x0,iState)
% General interface of MPC with C. In C code, we always call MPC1xx with
% different ID to specify which MPC we want to use. Different MPCs differ
% in cost function form, upper and lower bound. MPC1xx read from feedback
% and settings and setup constraints for fmincon().
%
% INPUTS:
%    feedback: 
%    settings:
%    PridictionHorizon:
%    ControlHorizon:
%    dt:
%    D_set:
%    D_factor:
%    R_set:
%    R_factor:
%    h_set:
%    h_factor:
%    v_set:
%    v_factor: 
%    weighting: a vector weights the cost at each steps.
%    Detail:
%    FileNameLog:
%    ID:
%    101: cost function penalize deviation of mean(R2), SOR and thickness
%         Implement final thickness requirement as a set point (*soft*
%         constraint), which means the thickness setthe lower bound 
%    102: cost function penalize deviation of mean(R2), SOR and thickness,
%         same as MPC101
%         Implement final thickness requirement as a *hard* constraint
%    103: cost function penalize deviation of variance control of R^2 from 0
%         Using numerical routine to calculate var(R2) in the model
%         No final thickness requirement
%    104: cost function is the same as 103, but use analytical formula for
%         var(R2)
%         No final thickness requirement
%    105: put variance as a constraint
% 
% OUTPUTS:
%    u: deposition rate of all the predicted future steps.
%    info:
%      Hopt:
%      Dopt:
%      R2opt:
%      varR2opt:
%      Uopt:
%      cost:
%      exitflag:
%    oState: structure representing updated open-loop model
% AUTHOR: Xinyu Zhang (zxy1256@gmail.com)
% DATE: 2009.08.25

assert(isstruct(feedback),'feedback must be a structure');
assert(all(isfield(feedback,{'alpha','beta','rho','h'})));

assert(isstruct(iMPC),'iMPC must be a structure');
assert(all(isfield(iMPC,{'D_set','Fact_D',...
                         'R2_set','Fact_r2',...
                         'Ht_set','Fact_H',...
                         'P','dt','lb','ub','rt_dep'})));

assert(isscalar(x0),'x0 must be a scalar');

oMPC = iMPC;
oMPC.model = model_calstat(iMPC.model,feedback);
P  = iMPC.P;
ID = iMPC.ControllerTypeID;
assert((ID>=100 && ID<200),'ID must be 100<=ID<200');

% --------- Linear inequality constraints -----------
A = [];
b = [];
% --------- Linear equality constraints -------------
Aeq = [];
beq = [];
% --------- Upper and lower bound -------------------
lb = zeros(P,1);
ub = zeros(P,1);
if any(ID == [101,103,104,105])
    for i = 1:P
        lb_temp1 = x0-iMPC.dt*iMPC.rt_dep*i;
        lb(i) = max([lb_temp1,iMPC.lb]);   
        ub_temp1 = x0+iMPC.dt*iMPC.rt_dep*i;
        ub(i) = min([ub_temp1,iMPC.ub]);
    end
elseif any(ID == [102])
    for i = 1:P
        lb_temp1 = x0-iMPC.dt*iMPC.rt_dep*i;
        lb_temp2 = (iMPC.Ht_set-feedback.h)/(iMPC.t_end-iMPC.dt*feedback.t_idx);
        lb(i) = max([lb_temp1,lb_temp2,iMPC.lb]);
        ub_temp1 = x0+iMPC.dt*iMPC.rt_dep*i;
        ub(i) = min([ub_temp1,iMPC.ub]);
    end
else
    error('Lower and upper bound not initialized\n');
end
% --------- Nonlinear constraint --------------------
if any(ID == 105)
	nonlcon = @(y) nonlcon105(y,feedback,iMPC);
else
    nonlcon = [];
end


% -------- Initial value ----------------------------
% Implement the initial value mechanism in C code 
mpc0 = oMPC;
mpc0.P = 1;
cost_min = 1e30;
dep_ini = x0(1);
for v_dep_rate1=lb(1):(ub(1)-lb(1))/100:ub(1)
    cost_temp = objfun_b(v_dep_rate1,feedback,mpc0);
    if(cost_temp<cost_min)
        cost_min = cost_temp;
        dep_ini = v_dep_rate1;
    end
end
v_dep_rate = dep_ini*ones(P,1);

% --------- Solving the optimization problem --------
option = optimset('LargeScale','off','Display','off');
[x,fval,exitflag] = fmincon(@(y) objfun_b(y,feedback,oMPC),v_dep_rate,A,b,Aeq,beq,lb,ub,nonlcon,option);
assert(exitflag>=0,'Infeasible solution is encounterd');

% --------- Output ----------------------------------
% mpc.model = model_calstat(mpc.model,feedback);
% mpc.model.varAlpha2 = iState.varAlpha2;
% mpc.model.varBeta2  = iState.varBeta2;
% mpc.model.varR2     = iState.varR2;

model_pred = oMPC.model;
for i = 1:P
    model_pred = model_update(model_pred,x(i),oMPC.dt);
    info.Hopt(i)     = model_pred.h;
    info.Dopt(i)     = model_pred.rho;
    info.R2opt(i)    = model_pred.meanR2;
    info.varR2opt(i) = model_pred.varR2;
end
u = x(1);
info.Uopt     = x;
info.cost     = fval;
info.exitflag = exitflag;

oMPC.model = model_update(oMPC.model,x(1),oMPC.dt);



